#include "rosbag.h"
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <std_msgs/Int32.h>

#include "util/topicfiled.h"
#include "util/rosbag.h"



RosBagSaver::RosBagSaver()
{

}

bool RosBagSaver::readPath(const std::string &bag_path,  std::vector<nav_msgs::Path> &paths)
{
    try {
        // 1. 打开bag文件（只读模式）
        rosbag::Bag bag;
        bag.open(bag_path, rosbag::bagmode::Read);


        rosbag::View view(bag, rosbag::TopicQuery(TopicFiled::robotNumFiled));
        if(view.size() <= 0)
        {
            return false;
        }

        int robotNum = 0;
        auto m = view.begin();
        {
            std_msgs::Int32::ConstPtr msg = m->instantiate<std_msgs::Int32>();
            if(msg == nullptr)
            {
                return false;
            }

            robotNum = msg->data;
        }

        if(robotNum <=0){
            return false;
        }

        // 2. 定义遍历参数
        std::vector<std::string> topics;
        for(int i =0; i < robotNum; i++){
            std::string topic = "/robot" + std::to_string(i+1) + "/path";
            topics.push_back(topic);
        }


        // 3. 获取符合条件的消息视图
        rosbag::View view2(bag, rosbag::TopicQuery(topics));

        // 4. 遍历消息
        BOOST_FOREACH(rosbag::MessageInstance const m, view2) {
          // 4.1 解析具体消息类型
          nav_msgs::Path::ConstPtr msg = m.instantiate<nav_msgs::Path>();
          if (msg == nullptr) {
              return false;
            }

            paths.push_back(*msg);
          }

        bag.close();
      }
      catch (const rosbag::BagIOException& e) {
        ROS_ERROR("Bag IO error: %s", e.what());
        return false;
      }
      catch (const rosbag::BagException& e) {
        ROS_ERROR("Bag exception: %s", e.what());
        return false;
      }

    return true;
}

bool RosBagSaver::writePoint2(const std::string fileName, sensor_msgs::PointCloud2 &msg)
{
    rosbag::Bag bag;
    try {
      bag.open(fileName, rosbag::bagmode::Write);
    } catch (std::exception &e) {
        ROS_WARN("open paths %s filed",  fileName.c_str());
        return false;
    }

    bag.write(TopicFiled::point2Save, ros::Time::now(), msg);
    bag.close();

    return true;
}

bool RosBagSaver::readPoint2(const std::string &fileName, sensor_msgs::PointCloud2 &msg)
{
    try {
        // 1. 打开bag文件（只读模式）
        rosbag::Bag bag;
        bag.open(fileName, rosbag::bagmode::Read);


        rosbag::View view(bag, rosbag::TopicQuery(TopicFiled::point2Save));
        if(view.size() <= 0)
        {
            return false;
        }

        auto m = view.begin();
        {
            sensor_msgs::PointCloud2::ConstPtr tmp = m->instantiate<sensor_msgs::PointCloud2>();
            if(tmp == nullptr)
            {
                return false;
            }

            msg = *tmp;
        }

        bag.close();
      }
      catch (const rosbag::BagIOException& e) {
        ROS_ERROR("Bag IO error: %s", e.what());
        return false;
      }
      catch (const rosbag::BagException& e) {
        ROS_ERROR("Bag exception: %s", e.what());
        return false;
      }

    return true;
}

bool RosBagSaver::wirtePath(const std::string &bag_path, const std::vector<nav_msgs::Path> &paths)
{
    rosbag::Bag bag;
    try {
      bag.open(bag_path, rosbag::bagmode::Write);
    } catch (std::exception &e) {
        ROS_WARN("open paths %s filed",  bag_path.c_str());
        return false;
    }

    std_msgs::Int32 num;
    num.data =(int)paths.size();
    bag.write(TopicFiled::robotNumFiled.c_str(), ros::Time::now(),num);

    for(uint i = 0; i < paths.size(); i++){
        const auto& path =  paths[i];
        // 为每个路径添加唯一标识（可根据实际需求修改）
        std_msgs::Header header = path.header;
        header.stamp = ros::Time::now(); // 添加写入时间戳

        // 写入bag文件（topic名可根据需要修改）
        std::string topic = "/robot" + std::to_string(i+1) + "/path";
        bag.write(topic.c_str(), ros::Time::now(), path);
    }
    bag.close();
    ROS_INFO("Saved %zu paths to %s", paths.size(), bag_path.c_str());
    return true;
}
